#include <iostream>
#include <cstdlib>
#include <string>
#include <time.h>
#include <thread>
#include <queue>
#include <unistd.h>

#include "opencv2/opencv.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>  
#include <opencv2/imgproc/imgproc.hpp>  
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/video/tracking.hpp>

#include "nanotrack.hpp" 
#include "camera.hpp"

using namespace std;
using namespace cv;


void cxy_wh_2_rect(const cv::Point& pos, const cv::Point2f& sz, cv::Rect &rect)
{
    rect.x = cv::max(0, pos.x - int(sz.x / 2));
    rect.y = cv::max(0, pos.y - int(sz.y / 2));
    rect.width = int(sz.x);   
    rect.height = int(sz.y);  
}


int main(int argc, char** argv)
{
    // omp_set_num_threads(4);

    std::string T_backbone_model = "../models/track_backbone_T.rknn";
    std::string X_backbone_model = "../models/track_backbone_X.rknn";
    std::string head_model = "../models/head.rknn";
    const char* video_path = "/home/linaro/tracking/NanoTrack_Yolo/data/videos/person7.mp4"; 
    const char* savePath = "../data/result/people-7.mp4";
   
    // read camera
    // CameraVideoCapture capture(1920, 1080, "/dev/video8", 30);
    // cv::Mat frame = capture.getFrame();

    cv::VideoCapture video(video_path);
    cv::Mat frame;
    video.read(frame);

    // cv::VideoWriter trackWriter(savePath, cv::VideoWriter::fourcc('m','p', '4', 'v'), 30, cv::Size(frame.cols,frame.rows));

    //display
    cv::namedWindow("window", CV_WINDOW_NORMAL);
    cv::setWindowProperty("window", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);

    NanoTrack siam_tracker(T_backbone_model, X_backbone_model, head_model); 
    clock_t start = clock();  // clock()返回时间单位时ms


    cv::Rect trackWindow = cv::selectROI("window", frame); // 手动选择框
    //  cv::Rect trackWindow = cv::Rect(629,349,33,22);
    
    siam_tracker.init(frame, trackWindow);
    std::cout << "==========================" << std::endl;
    std::cout << "Init done!" << std::endl; 
    std::cout << std::endl; 

    
    // 置信度分数
    float cls_score_max = 0;  
    cv::Point2f s_x = {0.0f , 0.0f};
    pair<float, float> results;
    
    while (true)
    {
    
        // frame = capture.getFrame();    
        video >> frame;
        if(frame.empty())
        {
            // trackWriter.release();
            break;
        }
            start = clock();
            results = siam_tracker.track(frame); 
            cls_score_max = results.first;
            s_x = {results.second, results.second};
            // std::cout<<"fps : "<<CLOCKS_PER_SEC/double(clock()-start)<<std::endl;

            // Result to rect. 
            cv::Rect rect;
            cv::Rect search_rect;

            cxy_wh_2_rect(siam_tracker.state.target_pos, siam_tracker.state.target_sz, rect);
            cxy_wh_2_rect(siam_tracker.state.target_pos, s_x , search_rect);
        
            // cv::Mat track_window;
            cv::putText(frame,to_string(cls_score_max),cv::Point(rect.x,rect.y),cv::FONT_HERSHEY_SIMPLEX,1,cv::Scalar(0,0,255),2);
            cv::putText(frame,"Search Region", cv::Point(search_rect.x , search_rect.y), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255,255,0), 2);
            cv::rectangle(frame, rect, cv::Scalar(0, 255, 0), 2);
            cv::rectangle(frame, search_rect, cv::Scalar(255,0,0), 2);
                
        // std::cout<<"fps : "<<CLOCKS_PER_SEC/double(clock()-start)<<std::endl;

            //display
            cv::imshow("window", frame);
            cv::waitKey(1);
            // trackWriter.write(frame);
    }
    
    cv::destroyWindow("window");
    return 0;
}